/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Watchdog;

/**
 *
 * @author Mark
 */
public class MaintenanceMode
{
    private static Joystick driverJoy;

    private static REncoder flEnc;
    private static REncoder frEnc;
    private static REncoder rlEnc;
    private static REncoder rrEnc;

    private static Jaguar flDrive;
    private static Jaguar frDrive;
    private static Jaguar rlDrive;
    private static Jaguar rrDrive;

    private static Jaguar flTurn;
    private static Jaguar frTurn;
    private static Jaguar rlTurn;
    private static Jaguar rrTurn;

    private static int FRONT_LEFT = 1;
    private static int FRONT_RIGHT = 2;
    private static int REAR_LEFT = 3;
    private static int REAR_RIGHT = 4;

    public static void enterMaintenanceMode(
                                        Joystick stick,
                                        REncoder a_flEnc,
                                        REncoder a_frEnc,
                                        REncoder a_rlEnc,
                                        REncoder a_rrEnc,
                                        Jaguar a_flDrive,
                                        Jaguar a_frDrive,
                                        Jaguar a_rlDrive,
                                        Jaguar a_rrDrive,
                                        Jaguar a_flTurn,
                                        Jaguar a_frTurn,
                                        Jaguar a_rlTurn,
                                        Jaguar a_rrTurn)
    {

        flEnc = a_flEnc;
        frEnc = a_frEnc;
        rlEnc = a_rlEnc;
        rrEnc = a_rrEnc;

        flDrive = a_flDrive;
        frDrive = a_frDrive;
        rlDrive = a_rlDrive;
        rrDrive = a_rrDrive;

        flTurn = a_flTurn;
        frTurn = a_frTurn;
        rlTurn = a_rlTurn;
        rrTurn = a_rrTurn;
        driverJoy = stick;

        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "Maintenance mode entered.");
        DriverStationLCD.getInstance().updateLCD();

        pauseUntilReleased(Utils.MAINTENANCE_MODE_BUTTON_NUM);

        
        mainMenu();

        Utils.clearUserMessages();
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "Maintenance mode exited.");
        DriverStationLCD.getInstance().updateLCD();
    }

    private static void mainMenu()
    {
        boolean quitMM = false;
        boolean refreshMM = true;

        while(!quitMM)
        {
            Watchdog.getInstance().feed();
            if(refreshMM)
            {
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "Select one of the following with the joystick buttons.");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1, "7: Front-Left Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "8: Front-Right Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 1, "9: Rear-Left Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "10: Rear-Right Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "2: Exit Maintenance Mode");
                DriverStationLCD.getInstance().updateLCD();

                refreshMM = false;
            }

            if(driverJoy.getRawButton(7))
            {
                pauseUntilReleased(7);
                displaySubMenu(flDrive, flTurn, flEnc, "Front Left Wheel", false);

                refreshMM = true;
            }
            else if(driverJoy.getRawButton(8))
            {
                pauseUntilReleased(8);
                displaySubMenu(frDrive, frTurn, frEnc, "Front Right Wheel", true);

                refreshMM = true;
            }
            else if(driverJoy.getRawButton(9))
            {
                pauseUntilReleased(9);
                displaySubMenu(rlDrive, rlTurn, rlEnc, "Rear Left Wheel", false);
                refreshMM = true;
            }
            else if(driverJoy.getRawButton(10))
            {
                pauseUntilReleased(10);
                displaySubMenu(rrDrive, rrTurn, rrEnc, "Rear Right Wheel", true);

                refreshMM = true;
            }
            else if(driverJoy.getRawButton(1) || driverJoy.getRawButton(2))
            {
                pauseUntilReleased(1);
                pauseUntilReleased(2);

                quitMM = true;
            }
        }
        
    }

    /**
     * As long as the trigger or 2 have not been pressed, set the speed of the
     * motor as the speed of the joystick.
     * @param mtr Motor to set the speed of
     * @param enc Motor's encoder
     * @return True if the trigger was pressed, false if the cancel button was
     * pressed
     */
    private static void jogMotor(Jaguar mtr, REncoder enc, boolean steering, boolean invertMtrSpeed, String name)
    {
        double Angle = 0.0;
        double lastAngle = 0.0;

        if(!steering)
        {
            while(Math.abs(driverJoy.getY()) > .1)
            {
                Watchdog.getInstance().feed();
                if(!invertMtrSpeed)
                {
                    mtr.set(-driverJoy.getY());
                }
                else
                {
                    mtr.set(driverJoy.getY());
                }
            }
        }
        else
        {
            while(Math.abs(driverJoy.getTwist()) > .1)
            {
                Watchdog.getInstance().feed();
        //        System.out.println("Twisting wheel: " + driverJoy.getTwist());
                mtr.set(-driverJoy.getTwist());

                Angle = enc.getAngle();

                printEncoderValues(enc);
                if( Angle != lastAngle )
                {
                    printEncoderValues(enc);
                    lastAngle = Angle;
                }
            }
        }


        mtr.set(0); //just to be safe
    }

    /**
     * Calibrate wheel position for one of the wheels
     * @param mtr Motor to calibrate
     * @param enc Encoder of the motor
     * @param wheelPos Integer from the RecalibrationValues class that represents
     * which wheel is being calibrated
     */
    private static void zeroEncoder(REncoder enc)
    {
            enc.zeroAngle();
            DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "Encoder has been zeroed.");
            DriverStationLCD.getInstance().updateLCD();

    }

    /**
     * Print the raw value and current angle of the encoder.
     * @param enc Encoder
     */
    private static void printEncoderValues(REncoder enc)
    {
        System.out.println("-------------------------------");

        System.out.println("Current angle: " + Utils.StringFormat61f(enc.getAngle()));

        System.out.println("FL raw value : " + Utils.StringFormat6d(flEnc.getRawValue()));
        System.out.println("FR raw value : " + Utils.StringFormat6d(frEnc.getRawValue()));
        System.out.println("RL raw value : " + Utils.StringFormat6d(rlEnc.getRawValue()));
        System.out.println("RR raw value : " + Utils.StringFormat6d(rrEnc.getRawValue()));
    }

    /**
     * Display the submenu just for the turn motors
     * @param mtr Motor to tune
     * @param enc Encoder of the motor
     * @param name Name of the motor
     * @param wheelPos Integer from the RecalibrationValues
     */
    private static void displaySubMenu(Jaguar driveMtr, Jaguar steerMtr, REncoder enc, String name, boolean invertDriveMtr)
    {
        boolean exitSM = false;
        boolean refreshSM = true;
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, name);

        while(!exitSM)
        {
            Watchdog.getInstance().feed();
            if(refreshSM)
            {
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 6, "Select one of the following use the joystick buttons.");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 6, "Y direction movement: Jog drive motor");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 6, "Twist movement: Jog steering motor");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 6, "4: Zero wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 6, "2: Return to Main Menu");
                DriverStationLCD.getInstance().updateLCD();
                refreshSM = false;
            }
            if(Math.abs(driverJoy.getY()) > .1)
            {
                jogMotor(driveMtr, null, false, invertDriveMtr, name);
                refreshSM = true;
            }
            else if(Math.abs(driverJoy.getTwist()) > .1)
            {
                jogMotor(steerMtr, enc, true, invertDriveMtr, name);
                refreshSM = true;
            }
            else if(driverJoy.getRawButton(4))
            {
                pauseUntilReleased(4);
                zeroEncoder(enc);
                refreshSM = true;
            }
            if(driverJoy.getRawButton(2))
            {
                pauseUntilReleased(2);
                exitSM = true;
            }
        }
    }

    /**
     * Use this method when you are checking to see if a user has pressed a button
     * because you may exit the method and then that button will still be pressed.
     * This remains in an empty while loop as long as the button is pressed.
     * @param buttonNum
     */
    private static void pauseUntilReleased(int buttonNum)
    {
        while(driverJoy.getRawButton(buttonNum))
        {
            Watchdog.getInstance().feed();
            //wait until the user has released the current button
        }
    }

}
